# This file is used to launch rviz2 with the fishbot_model.urdf file
# 用于测试rviz的机器人可视化效果====》单纯机器人的建模验证

import os
from launch import LaunchDescription
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare


def generate_launch_description():
    package_name = 'fish_robot_model'
    urdf_name = "fishbot_model.urdf"

        # 获取包路径（带错误检查）
    try:
        pkg_share = FindPackageShare(package=package_name).find(package_name)
        if not pkg_share:
            raise RuntimeError(f"Package '{package_name}' not found")
    except Exception as e:
        raise RuntimeError(f"Package lookup failed: {e}")
    
    ld = LaunchDescription()
    urdf_model_path = os.path.join(pkg_share, f'urdf/{urdf_name}')

    robot_state_publisher_node = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        arguments=[urdf_model_path]
        )

    joint_state_publisher_node = Node(
        package='joint_state_publisher_gui',
        executable='joint_state_publisher_gui',
        name='joint_state_publisher_gui',
        arguments=[urdf_model_path]
        )

    rviz2_node = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        output='screen',
        )

    ld.add_action(robot_state_publisher_node)
    ld.add_action(joint_state_publisher_node)
    ld.add_action(rviz2_node)

    return ld
